from ament_index_python.packages import get_package_share_path
from hippo_common.launch_helper import (
    LaunchArgsDict,
    declare_vehicle_name_and_sim_time,
)
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode

from launch import LaunchDescription
from launch.actions import (
    DeclareLaunchArgument,
    IncludeLaunchDescription,
)
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration


def declare_launch_args(launch_description: LaunchDescription):
    declare_vehicle_name_and_sim_time(launch_description=launch_description)

    package_path = get_package_share_path('hippo_control')


    # 跟踪控制器配置文件
    tracking_path = str(
        package_path
        / 'config/tracking_controller_test.yaml'
    )
    action = DeclareLaunchArgument(
        'tracking_controller_config', default_value=tracking_path
    )
    launch_description.add_action(action)

    # 姿态控制器配置文件
    attitude_control_path = str(
        package_path / 'config/attitude_control/geometric_hippocampus_default.yaml'
    )
    action = DeclareLaunchArgument(
        'attitude_control_config', default_value=attitude_control_path
    )
    launch_description.add_action(action)

    


def add_composable_nodes(launch_description: LaunchDescription):
    nodes = []
    args = LaunchArgsDict()
    args.add_vehicle_name_and_sim_time()

    # 表示启用进程内通信机制
    extra_args = [{'use_intra_process_comms': True}]


    node = ComposableNode(
        package='hippo_control',
        plugin='hippo_control::trajectory_tracking_bluerov::TrackingControllerBlueROVNode',
        namespace=LaunchConfiguration('vehicle_name'),
        name='tracking_controller_bluerov',
        parameters=[
            args,
            LaunchConfiguration('tracking_controller_config'),
        ],
        extra_arguments=extra_args,
    )
    nodes.append(node)
    
    node = ComposableNode(
        package='hippo_control',
        plugin='hippo_control::nodes::AttitudeControlBlueROVNode',
        namespace=LaunchConfiguration('vehicle_name'),
        name='attitude_controller_bluerov_test',
        parameters=[
            LaunchConfiguration('attitude_control_config'),
            args,
        ],
        extra_arguments=extra_args,
    )
    nodes.append(node)

    container = ComposableNodeContainer(
        name='path_follower_and_attitude_container',
        namespace=LaunchConfiguration('vehicle_name'),
        package='rclcpp_components',
        executable='component_container',
        composable_node_descriptions=nodes,
        output='screen',
        emulate_tty=True,
    )
    launch_description.add_action(container)





def generate_launch_description():
    launch_description = LaunchDescription()
    declare_launch_args(launch_description=launch_description)
    add_composable_nodes(launch_description=launch_description)

    return launch_description
